/*
 * mathutil.cpp
 *
 *@author: matt
 *  Created on: Jun 23, 2009
 *      Author: matt
 */

#include "mathutil.h"

bool closest_pnt_on_line(VECTOR2DF const p, LINE2DF const l, VECTOR2DF& r)
{
	float u = l.slope.x;
	float v = l.slope.y;
	float x = p.x - l.origin.x;
	float y = p.y - l.origin.y;
	float c = u * u + v * v;
	if (c == 0.0f)
		return false;
	float t = 1 / c * (v * y + u * x);
	if (t > l.length)
		t = l.length;
	if (t < 0)
		t = 0;
	r.x = t * u + l.origin.x;
	r.y = t * v + l.origin.y;
	return true;
}

float dist(VECTOR2DF const v1, VECTOR2DF const v2)
{
	float dx = v1.x - v2.x;
	float dy = v1.y - v2.y;
	return sqrtf(dx * dx + dy * dy);
}

void endpoints(const LINE2DF l, VECTOR2DF &v1, VECTOR2DF& v2)
{
	float e1x = l.origin.x;
	float e1y = l.origin.y;
	float norm = sqrtf(l.slope.x * l.slope.x + l.slope.y * l.slope.y);
	float e2x = l.origin.x + l.slope.x * l.length / norm;
	float e2y = l.origin.y + l.slope.y * l.length / norm;
	if (e1x > e2x)
	{
		v1.x = e1x;
		v1.y = e1y;
		v2.x = e2x;
		v2.y = e2y;
		return;
	}
	else if (e1x == e2x)
	{
		if (e1y > e2y)
		{
			v1.x = e1x;
			v1.y = e1y;
			v2.x = e2x;
			v2.y = e2y;
			return;
		}
		else
		{
			v1.x = e2x;
			v1.y = e2y;
			v2.x = e1x;
			v2.y = e1y;
			return;
		}
	}
	else
	{
		v1.x = e2x;
		v1.y = e2y;
		v2.x = e1x;
		v2.y = e1y;
		return;
	}
}

//obsolete
//void line(const VECTOR2DF e1, const VECTOR2DF e2, LINE2DF& l)
//{
//	l.length = dist(e1, e2);
//	l.origin.x = e1.x;
//	l.origin.y = e1.y;
//	l.slope.x = e2.x - e1.x;
//	l.slope.y = e2.y - e1.y;
//}


/*Implementation of LINE2DF class*/

void LINE2DF::normalize()
{
	float norm = sqrtf(slope.x * slope.x + slope.y * slope.y);
	slope.x /= norm;
	slope.y /= norm;
}

bool LINE2DF::intersect(LINE2DF const l, VECTOR2DF& ve)
{
	float u1 = slope.x;
	float v1 = slope.y;
	float u2 = l.slope.x;
	float v2 = l.slope.y;
	float c = u1 * v2 - u2 * v1;
	if (c == 0)
		return false; //parallel
	float t = 1 / c * (v1 * (l.origin.x - origin.x) - u1 * (l.origin.y - origin.y));
	if (v1 == 0 && u1 == 0)
		return false; //not a line
	float s = -1;
	if (v1 != 0)
		s = 1 / v1 * (v2 * t + l.origin.y - origin.y);
	else
		s = 1 / u1 * (u2 * t + l.origin.x - origin.x);
	if (t >= 0 && t <= l.length && s >= 0 && s <= length)
	{
		ve.x = u1 * s + origin.x;
		ve.y = v1 * s + origin.y;
		return true;
	}
	return false;
}

void normalize(VECTOR2DF & v){
	float c = sqrt(dot(v,v));
	v.x /=c;
	v.y /=c;
}

float angle(VECTOR2DF const v1, VECTOR2DF const v2)
{
	float c1 = sqrtf(dot(v1, v1));
	float c2 = sqrtf(dot(v2, v2));
	VECTOR2DF vv1 =
	{ v1.x / c1, v1.y / c1 };
	VECTOR2DF vv2 =
	{ v2.x / c2, v2.y / c2 };
	return acos(dot(vv1, vv2));
}

float dot(VECTOR2DF const v1, VECTOR2DF const v2)
{
	return v1.x * v2.x + v1.y * v2.y;
}

void polar_to_cartesian(POLARDCOORD2DF const p, VECTOR2DF &v)
{
	v.x = p.r * cos(p.theta);
	v.y = p.r * sin(p.theta);
}

void cartesian_to_polar(VECTOR2DF const v, POLARDCOORD2DF & p)
{
	p.r = sqrt(v.x * v.x + v.y * v.y);
	if (v.x == 0 && v.y == 0)
		p.theta = 0.0f;
	else if (v.x >= 0)
		p.theta = asin(v.y / p.r);
	else if (v.x < 0)
		p.theta = -asin(v.y / p.r) + M_PI;
}

